#include <fstream>
#include <set>

#include <boost/filesystem.hpp>

#include "VO_ASM.h"


using namespace std;

/** Default Constructor */
VO_ASM::VO_ASM()
{
	this->init();
}


/** Initialization */
void VO_ASM::init()
{
	this->m_CVMShapes								= NULL;
	this->m_CVMAlignedShapes						= NULL;
	this->m_CVMAlignedShapesProject2Truncated		= NULL;
	this->m_CVMTruancatedAlignedShapesMean			= NULL;
	this->m_CVMTruancatedAlignedShapesSD			= NULL;
	this->m_CVMAlignedShapesMean					= NULL;
	this->m_CVMAlignedShapesSD						= NULL;
	this->m_CVMAlignedShapesEigenVectors			= NULL;
	this->m_CVMAlignedShapesEigenValues				= NULL;
	this->m_CVMTruncatedAlignedShapesEigenVectors	= NULL;
	this->m_CVMTruncatedAlignedShapesEigenValues	= NULL;
	this->m_CVMSimilarityTransformMatrix			= NULL;
	this->m_CVMPoints								= NULL;
	this->m_CVMConvexHull							= NULL;
	this->m_iNbOfSamples							= 0;
	this->m_iNbOfPoints								= 0;
	this->m_iNbOfShapes								= 0;
	this->m_iNbOfTruncatedShapes					= 0;
	this->m_iNbOfEigenShapesAtMost					= 0;
	this->m_fAverageShapeSize                       = 0.0f;
	this->m_iNbOfPointsOnConvexHull					= 0;
	this->m_iNbOfEdges								= 0;
	this->m_iNbOfTriangles							= 0;
	this->m_fTruncatedPercent_Shape                 = 0.95f;

}


/** Destructor */
VO_ASM::~VO_ASM()
{
	if (this->m_CVMShapes)								cvReleaseMat (&this->m_CVMShapes);
	if (this->m_CVMAlignedShapes)						cvReleaseMat (&this->m_CVMAlignedShapes);
	if (this->m_CVMAlignedShapesProject2Truncated)		cvReleaseMat (&this->m_CVMAlignedShapesProject2Truncated);
	if (this->m_CVMTruancatedAlignedShapesMean)			cvReleaseMat (&this->m_CVMTruancatedAlignedShapesMean);
	if (this->m_CVMTruancatedAlignedShapesSD)			cvReleaseMat (&this->m_CVMTruancatedAlignedShapesSD);
	if (this->m_CVMAlignedShapesMean)					cvReleaseMat (&this->m_CVMAlignedShapesMean);
	if (this->m_CVMAlignedShapesSD)						cvReleaseMat (&this->m_CVMAlignedShapesSD);
	if (this->m_CVMAlignedShapesEigenVectors)			cvReleaseMat (&this->m_CVMAlignedShapesEigenVectors);
	if (this->m_CVMAlignedShapesEigenValues)			cvReleaseMat (&this->m_CVMAlignedShapesEigenValues);
	if (this->m_CVMTruncatedAlignedShapesEigenVectors)	cvReleaseMat (&this->m_CVMTruncatedAlignedShapesEigenVectors);
	if (this->m_CVMTruncatedAlignedShapesEigenValues)	cvReleaseMat (&this->m_CVMTruncatedAlignedShapesEigenValues);
	if (this->m_CVMSimilarityTransformMatrix)			cvReleaseMat (&this->m_CVMSimilarityTransformMatrix);
	if (this->m_CVMPoints)								cvReleaseMat (&this->m_CVMPoints);
	if (this->m_CVMConvexHull)							cvReleaseMat (&this->m_CVMConvexHull);
}



/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Align all shapes before PCA

 @param	    vShapes         Input - all shapes before alignment

 @param	    alignedShapes   Output - all shapes after alignment

 @return     float           return average shape size of all shapes

 @note       make sure all alignedShapes, meanshape as well, are of size "1" !!!

 */
float VO_ASM::VO_AlignAllShapes(const vector<VO_AAMShape2D>& vShapes, vector<VO_AAMShape2D>& alignedShapes)
{
	unsigned int numberOfSamples = vShapes.size();
	unsigned int numberOfPoints  = vShapes[0].GetSize();
	float averageShapeSize       = 0.0f;

	alignedShapes = vShapes;

	VO_AAMShape2D meanAlignedShape;
	meanAlignedShape.SetSize(numberOfPoints);

	// Instead of Normalising all shapes to unit size, which makes some shape loose their actual scale, we compute the average shape norm first and then normalise all shapes based on this average shape size -- Salil 09/01/10	
	for(unsigned int i = 0; i < numberOfSamples; i++)
	{   
		averageShapeSize += alignedShapes[i].NormalizeNoScale(); // move to origin and scale to unit size (scale to unit size is not desirable because the relative scale of the shape is lost. Thus, it has been removed -- Salil Deena 11/01/10)
	}

	averageShapeSize /= numberOfSamples;    // Why this is the average shape size without minor errors? 
	// Because later do-while loop doesn't change the normalized shape size

	for(unsigned int i = 0; i < numberOfSamples; i++)
	{
		alignedShapes[i].Scale(1/averageShapeSize); // move to origin and scale to unit size
	}	


	VO_ASM::VO_CalcMeanShape( alignedShapes, meanAlignedShape );
	meanAlignedShape.SetHostImage("");

	// do a number of alignment iterations until the mean shape estimate is stable
	float diff, diff_max = FLT_EPSILON;
	int max_iter = EPOCH, iter = 1;
	VO_AAMShape2D tempMeanShape;
	float theta;

	CvMat * rot = cvCreateMat (1, numberOfSamples, CV_32FC1);

	do
	{
		// normalize and align all other shapes to the mean shape estimate
		for(unsigned int i=0;i < numberOfSamples;i++)
		{
			// cout << "Aligning shape " << i + 1 << endl;
			// align the i-th shape to the estimate of the mean shape
			alignedShapes[i].AlignToNoScale( meanAlignedShape, &theta );
			CV_MAT_ELEM( *rot, float, 0, i )  = theta;  // record the rotation

			// Rotate the shape so as to make the 0th and 10th shape to form a horizontal line -- Need to parameterise the indices later -- Salil 11/01/10	
			alignedShapes[i].AlignPointsHorizontal(0,14); 				

			// We no longer need that since the align does not take shape into account -- Salil 09/10/09	
			// re-scale to unit size to avoid the so-called 'shrinking effect'
			// i.e. the alignment error goes towards zero, when the shapes are downscaled
			//alignedShapes[i].Scale( 1.0f/alignedShapes[i].GetShapeNorm () );
			//alignedShapes[i].Scale( 1.0f/averageShapeSize );	
		}
		tempMeanShape = meanAlignedShape;

		// estimate the new mean shape
		VO_ASM::VO_CalcMeanShape( alignedShapes, meanAlignedShape );

		// if first iteration, adjust the orientation of the mean shape, so the rotation of the training set to the mean shape is 
		// -- on average -- zero or put more clearly: "make the meanshape have a mean orientation"
		if (iter==1)
		{
			CvScalar rotMean = cvAvg( rot );
			meanAlignedShape.Rotate ( (float) ( -rotMean.val[0] ) );
		}

		diff = (tempMeanShape-meanAlignedShape).GetShapeSize();

		++iter;

	}while( fabs(diff)/meanAlignedShape.GetShapeSize() > diff_max && iter < max_iter );

	cvReleaseMat (&rot);

	return averageShapeSize;

}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Rescale the alignedshapes so that meanshape's 2-norm could be "1"

 @param      meanAlignedShape    Input - meanalignedshape that all alignedshapes should rescale to

 @param      alignedShapes       Input and Output - aligned shape and aligned rescaled shape

 */
float VO_ASM::VO_RescaleAllAlignedShapes(const VO_AAMShape2D& meanAlignedShape,
vector<VO_AAMShape2D>& alignedShapes)
{
	unsigned int NbOfSamples = alignedShapes.size();
	float average_ts = 0.0f;

	// Explained by JIA Pei. 2008-03-04. scale back so that the mean shape size is equal to 1.0
	for(unsigned int i = 0; i < NbOfSamples; i++)
	{
		float ts = sqrt (alignedShapes[i]*meanAlignedShape);
		//cout << "ts = " << ts << endl;
		average_ts += ts;
		alignedShapes[i].Scale( 1.0f/ts );
	}

	return average_ts / NbOfSamples;
}



/**

 @author     JIA Pei

 @version    2008-03-24

 @brief      Rescale the alignedShape to the already trained meanAligneShape

 @param      meanAlignedShape    Input - meanalignedshape that all alignedshapes should rescale to

 @param      alignedShape        Input and Output - aligned shape and aligned rescaled shape

 */
void VO_ASM::VO_RescaleOneAlignedShape(const VO_AAMShape2D& meanAlignedShape, VO_AAMShape2D& alignedShape)
{
	float ts = sqrt (alignedShape*meanAlignedShape);
	alignedShape.Scale( 1.0f/ts );
}



/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Calculate mean shape

 @param      vShapes     Input - all shapes

 @param      meanShape   Output - mean shape

 */
void VO_ASM::VO_CalcMeanShape(const vector<VO_AAMShape2D>& vShapes, VO_AAMShape2D &meanShape)
{
	unsigned int numberOfSamples    = vShapes.size();
	unsigned int numberOfShapes     = vShapes[0].GetSize();
	meanShape.SetSize(numberOfShapes);
	meanShape = vShapes[0];
	meanShape = 0;

	for(unsigned int i = 0; i < numberOfSamples; i++)
	{
		meanShape += vShapes[i];
	}

	meanShape = meanShape / (float)numberOfSamples;

}




/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Judge is point "pt" inside the convex hull "ch"

 @param      pt              Input - the point

 @param      ch              Input - convex hull

 @param      includingHull	Input flag - whether including the boundary

 @return     bool            including or excluding

 */
bool VO_ASM::VO_IsPointInConvexHull(CvPoint2D32f pt, const CvMat* ch, bool includingHull)
{
	if(!includingHull)
	{
		if (cvPointPolygonTest( ch, pt, 0 ) > 0.0 )
			return true;
	}
	else
	{
		if (cvPointPolygonTest( ch, pt, 0 ) >= 0.0)
			return true;
	}

	return false;
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Judge whether edge indexed by (ind1+ind2) is already in the vector of "edges"

 @param      edges           edge collection

 @param      ind1            first index of the edge to be judged

 @param      ind2	        second index of the edge to be judged

 @return     bool            counted or not

 */
bool VO_ASM::VO_IsEdgeCounted(const vector<VO_AAMEdge>& edges, int ind1, int ind2)
{
	for (unsigned int i = 0; i < edges.size (); i++)
	{
		if ( ( (edges[i].GetIndex1() == ind1) && (edges[i].GetIndex2() == ind2) )
		|| ( (edges[i].GetIndex1() == ind2) && (edges[i].GetIndex2() == ind1) ) )
		{
			return true;
		}
	}

	return false;
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Judge whether triangle t is already in the vector of "triangles".

 @param      triangles       triangle collection

 @param      t               triangle to be judged

 */
bool VO_ASM::VO_IsTriangleCounted(const vector<VO_AAMTriangle2D> &triangles, const vector<int> t)
{
	set<int> tTriangle;
	set<int> sTriangle;

	for (unsigned int i = 0; i < triangles.size (); i ++)
	{
		// These two clear() are very important, cannot be displaced by empty().
		tTriangle.clear ();
		sTriangle.clear ();
		for (unsigned int j = 0; j < 3; j++ )
		{
			tTriangle.insert (triangles[i].GetVertexIndex(j) );
			sTriangle.insert (t[j]);
		}
		if (tTriangle == sTriangle)
		{
			return false;
		}
	}

	return true;
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Build convex hull

 @param      vertexes        Input - the point

 @param      ch              Output - convex hull

 @return     int             number of edges on the convex hull 

 */
int VO_ASM::VO_BuildConvexHull(const CvMat* vertexes, CvMat* ch)
{
	cvConvexHull2(vertexes, ch, CV_CLOCKWISE, 0 );

	return ch->cols;
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Build AAM Edges

 @param      vertexes            Input - all vertexes/points composing the shape

 @param      Subdiv              Input - sub division which is already computed beforehand

 @param      outEdges	        Output - AAM edges

 @return     int                 Number of edges

 */
int VO_ASM::VO_BuildEdges(const CvMat* vertexes, const CvSubdiv2D* Subdiv, vector<VO_AAMEdge>& outEdges)
{
	unsigned int NbOfPoints = vertexes->cols;
	CvSeqReader  reader;

	cvStartReadSeq( (CvSeq*)(Subdiv->edges), &reader, 0 );

	for( unsigned int i = 0; i < Subdiv->edges->total; i++ )
	{
		CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);

		if( CV_IS_SET_ELEM( edge ))
		{
			CvPoint2D32f org;
			CvPoint2D32f dst;

			CvSubdiv2DPoint* org_pt = cvSubdiv2DEdgeOrg((CvSubdiv2DEdge)edge);
			CvSubdiv2DPoint* dst_pt = cvSubdiv2DEdgeDst((CvSubdiv2DEdge)edge);

			if( org_pt && dst_pt )
			{
				org = org_pt->pt;
				dst = dst_pt->pt;

				for (unsigned int j = 0; j < NbOfPoints; j++)
				{
					// if the current edge convex (org points, not the dst point) is in our point list
					if ( (fabs ( org.x - CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, j ).x ) < FLT_EPSILON )
					&& ( fabs ( org.y - CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, j ).y ) < FLT_EPSILON ) )
					{
						for (unsigned int k = 0; k < NbOfPoints; k++)
						{
							// With the above org point, we search around for the dst point(s), 
							// which make org-dst an edge during cvSubdivDelaunay2DInsert()
							if ( ( fabs (dst.x - CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, k ).x ) < FLT_EPSILON )
							&& ( fabs (dst.y - CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, k ).y ) < FLT_EPSILON ) )
							{
								// Already tested, this->m_vAAMEdge is definitely correct!
								outEdges.push_back ( VO_AAMEdge(j,k) );
							}
						}
					}
				}
			}
		}

		CV_NEXT_SEQ_ELEM( Subdiv->edges->elem_size, reader );
	}

	return outEdges.size();
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Build AAM Triangles

 @param      edges               Input - to build triangles

 @return     int                 Number of triangles

 */
int VO_ASM::VO_BuildTriangles(const CvMat* vertexes, const vector<VO_AAMEdge> &edges, vector<VO_AAMTriangle2D> &outTriangles)
{
	outTriangles.clear();

	unsigned int NbOfPoints = vertexes->cols;
	for (unsigned int i = 0; i < edges.size (); i++)
	{
		int ind1 = edges[i].GetIndex1();
		int ind2 = edges[i].GetIndex2();

		for (unsigned int j = 0; j < NbOfPoints; j++)
		{
			// At most, there are only 2 triangles that can be added 
			if( VO_ASM::VO_IsEdgeCounted(edges, ind1, j) && VO_ASM::VO_IsEdgeCounted(edges, ind2, j) )
			{
				vector<CvPoint2D32f> vVertexes;
				vector<int> iVertexes;
				vVertexes.resize (3);
				iVertexes.resize (3);
				vVertexes[0] = CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, ind1 );
				vVertexes[1] = CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, ind2 );
				vVertexes[2] = CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, j );
				iVertexes[0] = ind1;
				iVertexes[1] = ind2;
				iVertexes[2] = j;
				if (VO_ASM::VO_IsTriangleCounted(outTriangles, iVertexes) )
				{
					outTriangles.push_back (VO_AAMTriangle2D(vVertexes, iVertexes));
				}
			}
		}
	}

	return outTriangles.size();
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Calculation triangulation mesh
 For IMM 58 points dataset, 19 edges on the convex hull, 133 inner edges.
 Totally, 133+19=152 edges
 (133*2+19)/3=95 triangles

 */
void VO_ASM::VO_BuildTemplateMesh(const CvMat* vertexes, CvMat* ch, vector<VO_AAMEdge> & edges, vector<VO_AAMTriangle2D> & triangles )
{
	unsigned int NbOfPoints = vertexes->cols;

	// Build Convex Hull
	unsigned int NbOfEdgesOnConvexHull = VO_ASM::VO_BuildConvexHull(vertexes, ch);

	//////////////////////////////////////////////////////////////////////////
	// Build Delaunay Triangulation Sub Divisions
	// Later VO_BuildEdges need DTSubdiv information
	CvSubdiv2D* tempCVSubdiv = NULL;

	CvRect rect = cvBoundingRect( ch, 0 );
	//CvRect rect = { 0, 0, 288, 302 };

	CvMemStorage* DelaunayStorage = cvCreateMemStorage(0);

	tempCVSubdiv = VO_ASM::init_delaunay( DelaunayStorage, rect );

	// cout << "So far so good again 2" << endl;
	// cout << "NbOfPoints = " << NbOfPoints << endl;

	// By JIA Pei, 2006-09-20. How to release this storage?
	//tempCVSubdiv = cvCreateSubdivDelaunay2D( rect, DelaunayStorage );

	CvPoint2D32f tempPoint;

	//cout << "VO_BuildTemplateMesh" << endl;
	for( unsigned int i = 0; i < NbOfPoints; i++ )
	{	
		tempPoint = CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, i );
		cvSubdivDelaunay2DInsert( tempCVSubdiv, CV_MAT_ELEM( *vertexes, CvPoint2D32f, 0, i ));	
	}

	// cout << "So far so good again 3" << endl;
	//exit(1);

	//////////////////////////////////////////////////////////////////////////

	unsigned int NbOfEdges = VO_ASM::VO_BuildEdges(vertexes, tempCVSubdiv, edges);
	unsigned int NbOfTriangles = VO_ASM::VO_BuildTriangles (vertexes, edges, triangles);

	// How to release CvSubdiv2D* m_CVSubdiv is still a problem.
	if (tempCVSubdiv)	cvClearSubdivVoronoi2D( tempCVSubdiv );
	cvReleaseMemStorage( &DelaunayStorage );

}

/** 
 Author Salil Deena
 Version 17/09/2009
 **/
CvSubdiv2D* VO_ASM::init_delaunay( CvMemStorage* storage, CvRect rect )
{
	CvSubdiv2D* subdiv;

	subdiv = cvCreateSubdiv2D( CV_SEQ_KIND_SUBDIV2D, sizeof(*subdiv),
	sizeof(CvSubdiv2DPoint),
	sizeof(CvQuadEdge2D),
	storage );
	cvInitSubdivDelaunay2D( subdiv, rect );

	return subdiv;
}

/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Calculate Shape Boundary

 @param      the shape

 @return     bool

 */
CvRect VO_ASM::VO_CalcShapeBoundaryRect(const VO_AAMShape2D& iShape)
{
	CvRect rect;

	unsigned int NbOfPoints =  iShape.GetSize();
	CvMat* tempPoints = cvCreateMat (1, NbOfPoints, CV_32FC2);

	for (unsigned int i = 0; i < NbOfPoints; i++)
	{
		CV_MAT_ELEM( *tempPoints, CvPoint2D32f, 0, i ) = iShape.GetPoint(i).GetXY();
	}

	rect = cvBoundingRect( tempPoints, 1 );
	cvReleaseMat(&tempPoints);

	return rect;
}



/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Is iShape inside img

 @return     bool

 */
bool VO_ASM::VO_IsShapeInsideImage(const VO_AAMShape2D& iShape, const IplImage* img)
{
	CvRect rect = VO_ASM::VO_CalcShapeBoundaryRect(iShape);

	if ( (rect.x >= 0) && (rect.y >= 0) && ( (rect.x + rect.width) <= img->width )
	&& ( (rect.y + rect.height) <= img->height ) )
	{
		return true;
	}
	else
	{
		return false;
	}
}




/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Aligned shape to reference scale shape

 @param      inShape         Input - aligned shape

 @param      shapeSD         Input - shape standard deviation

 @param      outShape        Output - output reference shape

 */
void VO_ASM::VO_AlignedShape2ReferenceScale(const VO_AAMShape2D& inShape, float shapeSD, VO_AAMShape2D& outShape)
{
	outShape = const_cast<VO_AAMShape2D&> (inShape) * shapeSD;

	outShape.Translate( -outShape.MinX(), -outShape.MinY() );
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Reference scale texture back to aligned one

 @param      inShape         Input - reference shape

 @param      shapeSD         Input - shape standard deviation

 @param      outShape        Output - output aligned shape

 */
void VO_ASM::VO_ReferenceShapeBack2Aligned(const VO_AAMShape2D& inShape, float shapeSD, VO_AAMShape2D& outShape)
{
	outShape = inShape;

	// scale first
	outShape.Scale(1.0f/shapeSD );

	// centralize it
	float x, y;
	outShape.CenterOfGravity( x, y );
	outShape.Translate( -x, -y );

	// note: no rotation!!!
}



/**

 @author     JIA Pei

 @version    2008-03-20

 @brief      shape parameters back project to aligned shape

 @param      inP             Input - input shape parameters

 @return     VO_AAMShape2D   - the back projected shape

 */
VO_AAMShape2D VO_ASM::VO_SParamBackProjectToAlignedShape(CvMat* inP)
{
	CvMat* oShape = cvCreateMat(1, this->m_iNbOfShapes, CV_32FC1);

	// P back to shape
	cvBackProjectPCA(inP, this->m_CVMAlignedShapesMean, this->m_CVMTruncatedAlignedShapesEigenVectors, oShape);
	VO_AAMShape2D res   =   VO_AAMShape2D::VO_CvMat2AAMShape2D(oShape);

	cvReleaseMat(&oShape);

	return res;
}



/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      build ASM

 @param      vShapes     Input - all input shapes

 @param      TPShape     Input - truncated percentage for shape model
 */
void VO_ASM::VO_BuildASM(const vector<VO_AAMShape2D> &vShapes, float TPShape)
{
	this->m_iNbOfSamples            = vShapes.size();
	this->m_iNbOfPoints             = vShapes[0].GetSize();
	this->m_iNbOfShapes             = this->m_iNbOfPoints * 2;
	this->m_fTruncatedPercent_Shape = TPShape;
	this->m_vAAMShapes              = vShapes;

	//cout << "So far so good" << endl;
	// Calculate meanshape before alignment
	VO_ASM::VO_CalcMeanShape(this->m_vAAMShapes, this->m_VOAAMMeanShape); 

	// Align all shapes
	this->m_fAverageShapeSize = VO_ASM::VO_AlignAllShapes(this->m_vAAMShapes, this->m_vAAMAlignedShapes);
	VO_ASM::VO_CalcMeanShape(this->m_vAAMAlignedShapes, this->m_VOAAMMeanAlignedShapePrior);
	//this->m_fAverageShapeScalingFactor = VO_ASM::VO_RescaleAllAlignedShapes(this->m_VOAAMMeanAlignedShapePrior,this->m_vAAMAlignedShapes);
	// Calculate meanshape, now, bear in mind that all alignedshapes and the meanshape are all of size "1"
	VO_ASM::VO_CalcMeanShape(this->m_vAAMAlignedShapes, this->m_VOAAMMeanAlignedShapePost);     // actually, "1" already

	this->m_VOAAMMeanAlignedShapePost.Scale( 1.0f/this->m_VOAAMMeanAlignedShapePost.GetShapeNorm () );  // "1" again

	this->m_CVMShapes           = cvCreateMat(this->m_iNbOfSamples, this->m_iNbOfShapes, CV_32FC1);
	this->m_CVMAlignedShapes    = cvCreateMat(this->m_iNbOfSamples, this->m_iNbOfShapes, CV_32FC1);
	// VO_AAMShape2D2CvMat() is not available here. For a vector of VO_AAMShape2D
	for(unsigned int i=0; i < this->m_iNbOfSamples; i++)
	{
		for(unsigned int j = 0; j < this->m_iNbOfPoints; j++)
		{
			CV_MAT_ELEM( *this->m_CVMShapes, float, i, 2*j ) = this->m_vAAMShapes[i].GetPoint(j).GetXY().x;
			CV_MAT_ELEM( *this->m_CVMShapes, float, i, 2*j+1 ) = this->m_vAAMShapes[i].GetPoint(j).GetXY().y;

			CV_MAT_ELEM( *this->m_CVMAlignedShapes, float, i, 2*j ) = this->m_vAAMAlignedShapes[i].GetPoint(j).GetXY().x;
			CV_MAT_ELEM( *this->m_CVMAlignedShapes, float, i, 2*j+1 ) = this->m_vAAMAlignedShapes[i].GetPoint(j).GetXY().y;
		}
	}

	this->m_iNbOfEigenShapesAtMost = MIN(this->m_iNbOfSamples, this->m_iNbOfShapes);

	this->m_CVMAlignedShapesEigenValues     = cvCreateMat( 1, this->m_iNbOfEigenShapesAtMost, CV_32FC1 );
	this->m_CVMAlignedShapesEigenVectors    = cvCreateMat( this->m_iNbOfEigenShapesAtMost, this->m_iNbOfShapes, CV_32FC1 );

	// void cvCalcPCA( const CvArr* data, CvArr* avg, CvArr* eigenvalues, CvArr* eigenvectors, int flags );
	// According to OpenCV cvCalcPCA function description:
	// avg - The mean (average) vector, computed inside the function or provided by user.
	// Don't forget to specify m_CVMAlignedShapesMean before calculation
	this->m_CVMAlignedShapesMean = cvCreateMat( 1, this->m_iNbOfShapes, CV_32FC1 );
	this->m_CVMAlignedShapesSD   = cvCreateMat( 1, this->m_iNbOfShapes, CV_32FC1 );
	this->m_VOAAMMeanAlignedShapePost.VO_AAMShape2D2CvMat(this->m_CVMAlignedShapesMean);

	cvCalcPCA( this->m_CVMAlignedShapes, this->m_CVMAlignedShapesMean, this->m_CVMAlignedShapesEigenValues, 
	this->m_CVMAlignedShapesEigenVectors, CV_PCA_DATA_AS_ROW/* + CV_PCA_USE_AVG */);

	VO_Common::VO_CalcSamplesSDWithKnownMean(this->m_CVMAlignedShapes,this->m_CVMAlignedShapesMean, this->m_CVMAlignedShapesSD);

	CvScalar SumOfEigenValues = cvSum( this->m_CVMAlignedShapesEigenValues );

	float limit = (float) ( FLT_EPSILON * SumOfEigenValues.val[0] );

	unsigned int i = 0;
	for(i = 0; i < this->m_CVMAlignedShapesEigenValues->cols; i++)
	{ 
		if ( CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, float, 0, i ) < limit) break;
	}

	// NonZero EigenValues
	unsigned int NbOfNonZeroEigenValues = i;

	for(i = NbOfNonZeroEigenValues; i < this->m_iNbOfEigenShapesAtMost; i++)
	{
		CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, float, 0, i ) = 0.0f;
	}

	SumOfEigenValues = cvSum( this->m_CVMAlignedShapesEigenValues );
	float ps = 0.0f;
	this->m_iNbOfTruncatedShapes = 0;

	for(unsigned int i = 0; i < NbOfNonZeroEigenValues; i++)
	{
		ps += CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, float, 0, i );
		++this->m_iNbOfTruncatedShapes;
		if( ps/SumOfEigenValues.val[0] >= this->m_fTruncatedPercent_Shape) break;
	}

	// Set the Truncated Shape to 140
	// this->m_iNbOfTruncatedShapes = 140; 

	this->m_CVMTruncatedAlignedShapesEigenValues    = cvCreateMat( 1, this->m_iNbOfTruncatedShapes, CV_32FC1);
	this->m_CVMTruncatedAlignedShapesEigenVectors   = cvCreateMat( this->m_iNbOfTruncatedShapes, this->m_iNbOfShapes, CV_32FC1);

	for(unsigned int i = 0; i < this->m_iNbOfTruncatedShapes; i++)
	{
		CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenValues, float, 0, i )
			= CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, float, 0, i );

		for (unsigned int j = 0; j < this->m_iNbOfShapes; j++)
		{
			CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, float, i, j )
				= CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenVectors, float, i, j );
		}
	}

	this->m_CVMAlignedShapesProject2Truncated = cvCreateMat( this->m_iNbOfSamples, this->m_iNbOfTruncatedShapes, CV_32FC1);
	cvProjectPCA( this->m_CVMAlignedShapes, this->m_CVMAlignedShapesMean, this->m_CVMTruncatedAlignedShapesEigenVectors, 
	this->m_CVMAlignedShapesProject2Truncated );

	this->m_CVMTruancatedAlignedShapesMean  = cvCreateMat( 1, this->m_iNbOfTruncatedShapes, CV_32FC1);
	this->m_CVMTruancatedAlignedShapesSD    = cvCreateMat( 1, this->m_iNbOfTruncatedShapes, CV_32FC1);
	// This time, the mean value is not specified beforehand
	VO_Common::VO_CalcSamplesMeanNSD(this->m_CVMAlignedShapesProject2Truncated, this->m_CVMTruancatedAlignedShapesMean, this->m_CVMTruancatedAlignedShapesSD);

	// Calculate reference shape 
	VO_ASM::VO_AlignedShape2ReferenceScale(this->m_VOAAMMeanAlignedShapePost, this->m_fAverageShapeSize, this->m_VOAAMReferenceShape);

	// 1) Build template shape mesh
	this->m_CVMPoints = cvCreateMat (1, this->m_iNbOfPoints, CV_32FC2);
	this->m_CVMConvexHull = cvCreateMat (1, this->m_iNbOfPoints, CV_32FC2);

	for( unsigned int i = 0; i < this->m_iNbOfPoints; i++ )
	{
		CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, i ) = this->m_VOAAMReferenceShape.GetPoint(i).GetXY();
	}

	// cout << "So far so good again" << endl;
	//exit(1);

	VO_ASM::VO_BuildTemplateMesh(this->m_CVMPoints, this->m_CVMConvexHull, this->m_vAAMEdge, this->m_vAAMTriangle2D );
	//VO_ASM::VO_BuildTemplateMesh(this->m_CVMAlignedShapesMean, this->m_CVMConvexHull, this->m_vAAMEdge, this->m_vAAMTriangle2D );

	this->m_iNbOfPointsOnConvexHull     = this->m_CVMConvexHull->cols;
	this->m_iNbOfEdges                  = this->m_vAAMEdge.size();
	this->m_iNbOfTriangles              = this->m_vAAMTriangle2D.size();

	// 2) Calculate m_vAAMNormalizedTriangle2D - make sure the mesh is built first, so that we have m_iNbOfTriangles
	vector<CvPoint2D32f> tempVertexes;
	vector<int> iVertexes;
	tempVertexes.resize(3);
	iVertexes.resize(3);

	for (unsigned int i = 0; i < this->m_iNbOfTriangles; i++)
	{
		iVertexes[0] = this->m_vAAMTriangle2D[i].GetVertexIndex(0);
		iVertexes[1] = this->m_vAAMTriangle2D[i].GetVertexIndex(1);
		iVertexes[2] = this->m_vAAMTriangle2D[i].GetVertexIndex(2);
		tempVertexes[0].x = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[0] );
		tempVertexes[0].y = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[0] + 1);
		tempVertexes[1].x = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[1] );
		tempVertexes[1].y = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[1] + 1);
		tempVertexes[2].x = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[2] );
		tempVertexes[2].y = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[2] + 1);

		this->m_vAAMNormalizedTriangle2D.push_back (VO_AAMTriangle2D(tempVertexes, iVertexes));
	}



}



/**

 @author     JIA Pei

 @version    2008-03-18

 @brief      Save ASM to a specified folder

 @param      fn         Input - the folder that ASM to be saved to

 */
void VO_ASM::VO_Save(const string& fd)
{
	cout << "Saving ASM model ..." << endl;
	string fn = fd+"/ASM";
	if (!boost::filesystem::is_directory(fn) )
		boost::filesystem::create_directory( fn );

	fstream fp;
	string tempfn;

	// ASM
	tempfn = fn + "/ASM" + ".txt";
	fp.open(tempfn.c_str (), ios::out);

	// m_iNbOfSamples
	fp << "m_iNbOfSamples" << endl;
	fp << this->m_iNbOfSamples << endl;

	// m_iNbOfPoints
	fp << "m_iNbOfPoints" << endl;
	fp << this->m_iNbOfPoints << endl;

	// m_iNbOfShapes
	fp << "m_iNbOfShapes" << endl;
	fp << this->m_iNbOfShapes << endl;

	// m_iNbOfTruncatedShapes
	fp << "m_iNbOfTruncatedShapes" << endl;
	fp << this->m_iNbOfTruncatedShapes << endl;

	// m_iNbOfEigenShapesAtMost
	fp << "m_iNbOfEigenShapesAtMost" << endl;
	fp << this->m_iNbOfEigenShapesAtMost << endl;

	// m_fAverageShapeSize
	fp << "m_fAverageShapeSize" << endl;
	fp << this->m_fAverageShapeSize << endl;

	// m_iNbOfPointsOnConvexHull
	fp << "m_iNbOfPointsOnConvexHull" << endl;
	fp << this->m_iNbOfPointsOnConvexHull << endl;

	// m_iNbOfEdges
	fp << "m_iNbOfEdges" << endl;
	fp << this->m_iNbOfEdges << endl;

	// m_iNbOfTriangles
	fp << "m_iNbOfTriangles" << endl;
	fp << this->m_iNbOfTriangles << endl;

	// m_fTruncatedPercent_Shape
	fp << "m_fTruncatedPercent_Shape" << endl;
	fp << this->m_fTruncatedPercent_Shape << endl;

	// m_fAverageShapeScalingFactor
	fp << "m_fAverageShapeScalingFactor" << endl;
	fp << this->m_fAverageShapeScalingFactor << endl;

	fp.close();

	// m_CVMShapes
	tempfn = fn + "/m_CVMShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMShapes" << endl;
	for (unsigned int i = 0; i < this->m_CVMShapes->rows; i++)
	{
		for (unsigned int j = 0; j < this->m_CVMShapes->cols; j++)
		{
			fp << CV_MAT_ELEM( *this->m_CVMShapes, float, i, j) << " " ;
		}
		fp << endl;
	}
	fp.close();

	// m_CVMAlignedShapes
	tempfn = fn + "/m_CVMAlignedShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMAlignedShapes" << endl;
	for (unsigned int i = 0; i < this->m_CVMAlignedShapes->rows; i++)
	{
		for (unsigned int j = 0; j < this->m_CVMAlignedShapes->cols; j++)
		{
			fp << CV_MAT_ELEM( *this->m_CVMAlignedShapes, float, i, j) << " " ;
		}
		fp << endl;
	}
	fp.close();

	// m_CVMAlignedShapesProject2Truncated
	tempfn = fn + "/m_CVMAlignedShapesProject2Truncated" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMAlignedShapesProject2Truncated" << endl;
	for (unsigned int i = 0; i < this->m_CVMAlignedShapesProject2Truncated->rows; i++)
	{
		for (unsigned int j = 0; j < this->m_CVMAlignedShapesProject2Truncated->cols; j++)
		{
			fp << CV_MAT_ELEM( *this->m_CVMAlignedShapesProject2Truncated, float, i, j) << " " ;
		}
		fp << endl;
	}
	fp.close();

	// m_CVMTruancatedAlignedShapesMean
	tempfn = fn + "/m_CVMTruancatedAlignedShapesMean" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMTruancatedAlignedShapesMean" << endl;
	for (unsigned int i = 0; i < this->m_CVMTruancatedAlignedShapesMean->cols; i++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMTruancatedAlignedShapesMean, float, 0, i) << " " ;
	}
	fp << endl;
	fp.close();

	// m_CVMTruancatedAlignedShapesSD
	tempfn = fn + "/m_CVMTruancatedAlignedShapesSD" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMTruancatedAlignedShapesSD" << endl;
	for (unsigned int i = 0; i < this->m_CVMTruancatedAlignedShapesSD->cols; i++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMTruancatedAlignedShapesSD, float, 0, i) << " " ;
	}
	fp << endl;
	fp.close();

	// m_CVMAlignedShapesMean
	tempfn = fn + "/m_CVMAlignedShapesMean" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMAlignedShapesMean" << endl;
	for (unsigned int i = 0; i < this->m_CVMAlignedShapesMean->cols; i++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, i) << " " ;
	}
	fp << endl;
	fp.close();

	// m_CVMAlignedShapesSD
	tempfn = fn + "/m_CVMAlignedShapesSD" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMAlignedShapesSD" << endl;
	for (unsigned int i = 0; i < this->m_CVMAlignedShapesSD->cols; i++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMAlignedShapesSD, float, 0, i) << " " ;
	}
	fp << endl;
	fp.close();

	// m_CVMAlignedShapesEigenVectors
	tempfn = fn + "/m_CVMAlignedShapesEigenVectors" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMAlignedShapesEigenVectors" << endl;
	for (unsigned int i = 0; i < this->m_CVMAlignedShapesEigenVectors->rows; i++)
	{
		for (unsigned int j = 0; j < this->m_CVMAlignedShapesEigenVectors->cols; j++)
		{
			fp << CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenVectors, float, i, j) << " " ;
		}
		fp << endl;
	}
	fp.close();

	// m_CVMAlignedShapesEigenValues
	tempfn = fn + "/m_CVMAlignedShapesEigenValues" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMAlignedShapesEigenValues" << endl;
	for (unsigned int i = 0; i < this->m_CVMAlignedShapesEigenValues->cols; i++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, float, 0, i) << " " ;
	}
	fp << endl;
	fp.close();

	// m_CVMTruncatedAlignedShapesEigenVectors
	tempfn = fn + "/m_CVMTruncatedAlignedShapesEigenVectors" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMTruncatedAlignedShapesEigenVectors" << endl;
	for (unsigned int i = 0; i < this->m_CVMTruncatedAlignedShapesEigenVectors->rows; i++)
	{
		for (unsigned int j = 0; j < this->m_CVMTruncatedAlignedShapesEigenVectors->cols; j++)
		{
			fp << CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, float, i, j) << " " ;
		}
		fp << endl;
	}
	fp.close();

	// m_CVMTruncatedAlignedShapesEigenValues
	tempfn = fn + "/m_CVMTruncatedAlignedShapesEigenValues" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMTruncatedAlignedShapesEigenValues" << endl;
	for (unsigned int i = 0; i < this->m_CVMTruncatedAlignedShapesEigenValues->cols; i++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenValues, float, 0, i) << " " ;
	}
	fp << endl;
	fp.close();

	// m_CVMPoints
	tempfn = fn + "/m_CVMPoints" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMPoints" << endl;
	for (unsigned int j = 0; j < this->m_CVMPoints->cols; j++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j ).x << " " 
			<< CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j ).y << " " << endl;
	}
	fp.close();

	// m_CVMConvexHull
	tempfn = fn + "/m_CVMConvexHull" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_CVMConvexHull" << endl;
	for (unsigned int j = 0; j < this->m_CVMConvexHull->cols; j++)
	{
		fp << CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, j).x << " " 
			<< CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, j).y << " " << endl;
	}
	fp.close();

	// m_VOAAMReferenceShape
	tempfn = fn + "/m_VOAAMReferenceShape" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_VOAAMReferenceShape" << endl;
	for (unsigned int i = 0; i < this->m_VOAAMReferenceShape.GetSize(); i++)
	{
		fp << this->m_VOAAMReferenceShape.GetPoint(i).GetXY().x << " ";
		fp << this->m_VOAAMReferenceShape.GetPoint(i).GetXY().y << " " << endl;
	}
	fp.close();

	// m_vAAMShapes
	tempfn = fn + "/m_vAAMShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_vAAMShapes" << endl;
	for (unsigned int i = 0; i < this->m_vAAMShapes.size(); i++)
	{
		for (unsigned int j = 0; j < this->m_iNbOfPoints; j++)
		{
			fp << this->m_vAAMShapes[i].GetPoint(j).GetXY().x << " " 
				<< this->m_vAAMShapes[i].GetPoint(j).GetXY().y << " "
				<< this->m_vAAMShapes[i].GetPoint(j).GetPath() << " " 
				<< this->m_vAAMShapes[i].GetPoint(j).GetType() << " " 
				<< this->m_vAAMShapes[i].GetPoint(j).GetIndex() << " " 
				<< this->m_vAAMShapes[i].GetPoint(j).GetFrom() << " "
				<< this->m_vAAMShapes[i].GetPoint(j).GetTo() << " " << endl;
		}
		fp << endl;
	}
	fp.close();

	// m_vAAMAlignedShapes
	tempfn = fn + "/m_vAAMAlignedShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_vAAMAlignedShapes" << endl;
	for (unsigned int i = 0; i < this->m_vAAMAlignedShapes.size(); i++)
	{
		for (unsigned int j = 0; j < this->m_iNbOfPoints; j++)
		{
			fp << this->m_vAAMAlignedShapes[i].GetPoint(j).GetXY().x << " ";
			fp << this->m_vAAMAlignedShapes[i].GetPoint(j).GetXY().y << " ";
		}
		fp << endl;
	}
	fp.close();

	// m_VOAAMMeanAlignedShapePrior
	tempfn = fn + "/m_VOAAMMeanAlignedShapePrior" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_VOAAMMeanAlignedShapePrior" << endl;
	for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
	{
		fp << this->m_VOAAMMeanAlignedShapePrior.GetPoint(i).GetXY().x << " ";
		fp << this->m_VOAAMMeanAlignedShapePrior.GetPoint(i).GetXY().y << " ";
	}
	fp << endl;
	fp.close();

	// m_VOAAMMeanAlignedShapePost
	tempfn = fn + "/m_VOAAMMeanAlignedShapePost" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_VOAAMMeanAlignedShapePost" << endl;
	for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
	{
		fp << this->m_VOAAMMeanAlignedShapePost.GetPoint(i).GetXY().x << " ";
		fp << this->m_VOAAMMeanAlignedShapePost.GetPoint(i).GetXY().y << " ";
	}
	fp << endl;
	fp.close();

	// m_VOAAMMeanShape
	tempfn = fn + "/m_VOAAMMeanShape" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_VOAAMMeanShape" << endl;
	for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
	{
		fp << this->m_VOAAMMeanShape.GetPoint(i).GetXY().x << " ";
		fp << this->m_VOAAMMeanShape.GetPoint(i).GetXY().y << " ";
	}
	fp << endl;
	fp.close();

	// m_vAAMEdge
	tempfn = fn + "/m_vAAMEdge" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_vAAMEdge" << endl;
	for (unsigned int i = 0; i < this->m_vAAMEdge.size(); i++)
	{
		fp << this->m_vAAMEdge[i].GetIndex1() << " ";
		fp << this->m_vAAMEdge[i].GetIndex2() << " " << endl;
	}
	fp.close();

	// m_vAAMTriangle2D
	tempfn = fn + "/m_vAAMTriangle2D" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_vAAMTriangle2D" << endl;
	for (unsigned int i = 0; i < this->m_vAAMTriangle2D.size (); i++)
	{
		fp  << this->m_vAAMTriangle2D[i].GetVertexIndex(0) << " " 
			<< this->m_vAAMTriangle2D[i].GetVertex(0).x << " "
			<< this->m_vAAMTriangle2D[i].GetVertex(0).y << " "
			<< this->m_vAAMTriangle2D[i].GetVertexIndex(1) << " " 
			<< this->m_vAAMTriangle2D[i].GetVertex(1).x << " "
			<< this->m_vAAMTriangle2D[i].GetVertex(1).y << " "
			<< this->m_vAAMTriangle2D[i].GetVertexIndex(2) << " "
			<< this->m_vAAMTriangle2D[i].GetVertex(2).x << " "
			<< this->m_vAAMTriangle2D[i].GetVertex(2).y << " "
			<< this->m_vAAMTriangle2D[i].GetdD() << " " << endl; 
	}
	fp.close();

	// m_vAAMNormalizedTriangle2D
	tempfn = fn + "/m_vAAMNormalizedTriangle2D" + ".txt";
	fp.open(tempfn.c_str (), ios::out);
	fp << "m_vAAMNormalizedTriangle2D" << endl;
	for (unsigned int i = 0; i < this->m_vAAMNormalizedTriangle2D.size (); i++)
	{
		fp  << this->m_vAAMNormalizedTriangle2D[i].GetVertexIndex(0) << " " 
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertex(0).x << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertex(0).y << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertexIndex(1) << " " 
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertex(1).x << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertex(1).y << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertexIndex(2) << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertex(2).x << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetVertex(2).y << " "
			<< this->m_vAAMNormalizedTriangle2D[i].GetdD() << " " << endl; 
	}
	fp.close();

}

/**

 @author     DEENA Salil

 @version    2009-09-22

 @brief      Load ASM from a specified folder

 @param      fn         Input - the folder that ASM is to be loaded from

 */

void VO_ASM::VO_Load(const string& fd)
{
	string tempfn;	
	string line;
	ifstream fp;
	vector<string> tokens;
	vector<string> data;
	int num_rows, num_cols;
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	cout << "Loading ASM model data ..." << endl;

	string fn = fd+"/ASM";
	if (!boost::filesystem::is_directory(fn) )	
	{
		cout << "ASM directory not present...aborting." << endl;
		exit(1);
	}

	// Read the variables first
	tempfn = fn + "/ASM" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfSamples = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfPoints = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfShapes = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfTruncatedShapes = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfEigenShapesAtMost = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_fAverageShapeSize = atof(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfPointsOnConvexHull = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfEdges = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_iNbOfTriangles = atoi(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_fTruncatedPercent_Shape = atof(line.c_str());
	getline(fp, line);
	getline(fp, line);
	this->m_fAverageShapeScalingFactor = atof(line.c_str());

	fp.close();

	tempfn = fn + "/m_CVMShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::in);
	getline(fp, line);

	int i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];
	TokenizeIntoTokensOnly(data[0], tokens, " ");

	this->m_CVMShapes = cvCreateMat(this->m_iNbOfSamples, this->m_iNbOfShapes, CV_32FC1);

	for (unsigned int i = 0; i < this->m_CVMShapes->rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");			
		for (unsigned int j = 0; j < this->m_CVMShapes->cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMShapes, float, i, j) =  atof(tokens[j].c_str());
		}
	}	
	data.clear();
	fp.close();
	tokens.clear();	

	// m_CVMAlignedShapes
	tempfn = fn + "/m_CVMAlignedShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];
	TokenizeIntoTokensOnly(data[0], tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMAlignedShapes = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMAlignedShapes, float, i, j) =  atof(tokens[j].c_str());
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_CVMAlignedShapesProject2Truncated
	tempfn = fn + "/m_CVMAlignedShapesProject2Truncated" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];
	TokenizeIntoTokensOnly(data[0], tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMAlignedShapesProject2Truncated = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMAlignedShapesProject2Truncated, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	tokens.clear();
	fp.close();

	// m_CVMAlignedShapesSD
	tempfn = fn + "/m_CVMAlignedShapesSD" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];
	TokenizeIntoTokensOnly(data[0], tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMAlignedShapesSD = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMAlignedShapesSD, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	tokens.clear();
	fp.close();

	// m_CVMTruancatedAlignedShapesMean
	tempfn = fn + "/m_CVMTruancatedAlignedShapesMean" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMTruancatedAlignedShapesMean = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMTruancatedAlignedShapesMean, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_CVMTruancatedAlignedShapesSD	
	tempfn = fn + "/m_CVMTruancatedAlignedShapesSD" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMTruancatedAlignedShapesSD = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMTruancatedAlignedShapesSD, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_CVMAlignedShapesEigenVectors	
	tempfn = fn + "/m_CVMAlignedShapesEigenVectors" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMAlignedShapesEigenVectors = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenVectors, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_CVMAlignedShapesEigenValues	
	tempfn = fn + "/m_CVMAlignedShapesEigenValues" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMAlignedShapesEigenValues = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	//m_CVMTruncatedAlignedShapesEigenVectors	
	tempfn = fn + "//m_CVMTruncatedAlignedShapesEigenVectors" + ".txt";
		fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMTruncatedAlignedShapesEigenVectors = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	//m_CVMTruncatedAlignedShapesEigenValues	
	tempfn = fn + "//m_CVMTruncatedAlignedShapesEigenValues" + ".txt";
		fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_CVMTruncatedAlignedShapesEigenValues = cvCreateMat(num_rows, num_cols, CV_32FC1);

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenValues, float, i, j) =  atof(tokens[j].c_str());	
		}
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_CVMPoints
	tempfn = fn + "/m_CVMPoints" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_cols = data.size();
	num_rows = tokens.size();

	this->m_CVMPoints = cvCreateMat (1, num_cols, CV_32FC2);

	for (unsigned int j = 0; j < num_cols; j++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[j], tokens, " ");				
		for (unsigned int i = 0; i < num_rows; i++)
		{
			if (i == 0)	
				CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j ).x =  atof(tokens[i].c_str());

			if (i == 1)
				CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j ).y =  atof(tokens[i].c_str());

		}	
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_CVMConvexHull
	tempfn = fn + "/m_CVMConvexHull" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_cols = data.size();
	num_rows = tokens.size();

	this->m_CVMConvexHull = cvCreateMat (1, num_cols, CV_32FC2);

	for (unsigned int j = 0; j < num_cols; j++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[j], tokens, " ");				
		for (unsigned int i = 0; i < num_rows; i++)
		{
			if (i == 0)	
				CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, j ).x =  atof(tokens[i].c_str());

			if (i == 1)
				CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, j ).y =  atof(tokens[i].c_str());

		}	
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_VOAAMMeanShape
	tempfn = fn + "/m_VOAAMMeanShape" + ".txt";
	fp.open(tempfn.c_str (), ios::in);	
	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_VOAAMMeanShape.SetSize(num_cols/2);
	this->m_VOAAMMeanShape = 0;

	i = 0;
	tokens.clear();		
	TokenizeIntoTokensOnly(line, tokens, " ");		
	for (unsigned int j = 0; j < num_cols; j+=2)
	{		
		tempCvPoint2D32f.x = atof(tokens[j].c_str()); 
		tempCvPoint2D32f.y = atof(tokens[j+1].c_str());	

		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		m_VOAAMMeanShape.SetPoint(tempAAMPoint2d, i);	    	

		i++;
	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_VOAAMMeanAlignedShapePrior
	tempfn = fn + "/m_VOAAMMeanAlignedShapePrior" + ".txt";
	fp.open(tempfn.c_str (), ios::in);	
	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_VOAAMMeanAlignedShapePrior.SetSize(num_cols/2);
	this->m_VOAAMMeanAlignedShapePrior = 0;

	i = 0;
	tokens.clear();		
	TokenizeIntoTokensOnly(line, tokens, " ");		
	for (unsigned int j = 0; j < num_cols; j+=2)
	{		
		tempCvPoint2D32f.x = atof(tokens[j].c_str()); 
		tempCvPoint2D32f.y = atof(tokens[j+1].c_str());	

		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		m_VOAAMMeanAlignedShapePrior.SetPoint(tempAAMPoint2d, i);	    	

		i++;	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_VOAAMMeanAlignedShapePost
	tempfn = fn + "/m_VOAAMMeanAlignedShapePost" + ".txt";
	fp.open(tempfn.c_str (), ios::in);	
	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_VOAAMMeanAlignedShapePost.SetSize(num_cols/2);
	this->m_VOAAMMeanAlignedShapePost = 0;

	i = 0;
	tokens.clear();		
	TokenizeIntoTokensOnly(line, tokens, " ");		
	for (unsigned int j = 0; j < num_cols; j+=2)
	{		
		tempCvPoint2D32f.x = atof(tokens[j].c_str()); 
		tempCvPoint2D32f.y = atof(tokens[j+1].c_str());	

		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		m_VOAAMMeanAlignedShapePost.SetPoint(tempAAMPoint2d, i);	    	

		i++;	}
	data.clear();
	fp.close();
	tokens.clear();

	// m_VOAAMReferenceShape
	tempfn = fn + "/m_VOAAMReferenceShape" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line);

	i = 0;
	while (getline(fp, line))
	{
		data.push_back(line);
		i++;
	}

	// Obtain number of cols in data
	line = data[0];

	TokenizeIntoTokensOnly(line, tokens, " ");

	num_rows = data.size();
	num_cols = tokens.size();

	this->m_VOAAMReferenceShape.SetSize(num_rows);
	this->m_VOAAMReferenceShape = 0;

	for (unsigned int i = 0; i < num_rows; i++)
	{
		tokens.clear();		
		TokenizeIntoTokensOnly(data[i], tokens, " ");				
		for (unsigned int j = 0; j < num_cols; j++)
		{
			if (j == 0)		
				tempCvPoint2D32f.x = atof(tokens[j].c_str()); 

			if (j == 1)
				tempCvPoint2D32f.y = atof(tokens[j].c_str());

			tempAAMPoint2d.SetXY(tempCvPoint2D32f);
			m_VOAAMReferenceShape.SetPoint(tempAAMPoint2d, i);	
		}    	}

	data.clear();
	fp.close();
	tokens.clear();

	// m_vAAMShapes
	tempfn = fn + "/m_vAAMShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	this->m_vAAMShapes.resize(m_iNbOfSamples);

	for (int i = 0; i < this->m_iNbOfSamples; i++)
	{
		getline(fp, line);  

		for (int j = 0; j < this->m_iNbOfPoints; j++)
		{		
			getline(fp, line);  
			tokens.clear();		
			TokenizeIntoTokensOnly(line, tokens, " ");

			this->m_vAAMShapes[i].SetSize(m_iNbOfPoints);

			tempCvPoint2D32f.x = atof(tokens[0].c_str()); 
			tempCvPoint2D32f.y = atof(tokens[1].c_str());	    
			tempAAMPoint2d.SetXY(tempCvPoint2D32f);
			tempAAMPoint2d.SetPath(	atoi(tokens[2].c_str()) ); 
			tempAAMPoint2d.SetType(	atoi(tokens[3].c_str()) );   
			tempAAMPoint2d.SetFrom( atoi(tokens[4].c_str()) );           	        	
			tempAAMPoint2d.SetIndex( atoi(tokens[5].c_str()) ); 
			tempAAMPoint2d.SetTo( atoi(tokens[6].c_str()) );  	
			this->m_vAAMShapes[i].SetPoint(tempAAMPoint2d, j);	
		}
	}
	fp.close();
	tokens.clear();

	// m_vAAMAlignedShapes
	tempfn = fn + "/m_vAAMAlignedShapes" + ".txt";
	fp.open(tempfn.c_str (), ios::in);

	getline(fp, line); 

	this->m_vAAMAlignedShapes.resize(m_iNbOfSamples);

	for (int i = 0; i < this->m_iNbOfSamples; i++)
	{
		getline(fp, line);
		tokens.clear();		
		TokenizeIntoTokensOnly(line, tokens, " ");  

		for (int j = 0; j < this->m_iNbOfPoints; j++)
		{					
			this->m_vAAMAlignedShapes[i].SetSize(m_iNbOfPoints);

			tempCvPoint2D32f.x = atof(tokens[2*j].c_str()); 
			tempCvPoint2D32f.y = atof(tokens[2*j+1].c_str());	    
			tempAAMPoint2d.SetXY(tempCvPoint2D32f); 		
			this->m_vAAMAlignedShapes[i].SetPoint(tempAAMPoint2d, j);	
		}
	}
	fp.close();
	tokens.clear();

	this->m_CVMConvexHull = cvCreateMat (1, this->m_iNbOfPoints, CV_32FC2);

	// 1) Build template shape mesh
	VO_ASM::VO_BuildTemplateMesh(this->m_CVMPoints, this->m_CVMConvexHull, this->m_vAAMEdge, this->m_vAAMTriangle2D );

	// 2) Calculate m_vAAMNormalizedTriangle2D - make sure the mesh is built first, so that we have m_iNbOfTriangles
	vector<CvPoint2D32f> tempVertexes;
	vector<int> iVertexes;
	tempVertexes.resize(3);
	iVertexes.resize(3);

	this->m_CVMAlignedShapesMean = cvCreateMat( 1, this->m_iNbOfShapes, CV_32FC1 );
	this->m_VOAAMMeanAlignedShapePost.VO_AAMShape2D2CvMat(this->m_CVMAlignedShapesMean);

	for (unsigned int i = 0; i < this->m_iNbOfTriangles; i++)
	{
		iVertexes[0] = this->m_vAAMTriangle2D[i].GetVertexIndex(0);
		iVertexes[1] = this->m_vAAMTriangle2D[i].GetVertexIndex(1);
		iVertexes[2] = this->m_vAAMTriangle2D[i].GetVertexIndex(2);
		tempVertexes[0].x = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[0] );
		tempVertexes[0].y = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[0] + 1);
		tempVertexes[1].x = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[1] );
		tempVertexes[1].y = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[1] + 1);
		tempVertexes[2].x = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[2] );
		tempVertexes[2].y = CV_MAT_ELEM( *this->m_CVMAlignedShapesMean, float, 0, 2*iVertexes[2] + 1);

		this->m_vAAMNormalizedTriangle2D.push_back (VO_AAMTriangle2D(tempVertexes, iVertexes));
	}
}

